{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "ename": "AttributeError",
     "evalue": "module 'book_format' has no attribute 'load_style'",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mAttributeError\u001b[0m                            Traceback (most recent call last)",
      "\u001b[1;32m<ipython-input-1-ec44a09e3350>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m\u001b[0m\n\u001b[0;32m      8\u001b[0m \u001b[1;31m# consistant with that look and feel.\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      9\u001b[0m \u001b[1;32mimport\u001b[0m \u001b[0mbook_format\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m---> 10\u001b[1;33m \u001b[0mbook_format\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mload_style\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mdirectory\u001b[0m\u001b[1;33m=\u001b[0m\u001b[1;34m'..'\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m     11\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m     12\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;31mAttributeError\u001b[0m: module 'book_format' has no attribute 'load_style'"
     ]
    }
   ],
   "source": [
    "from __future__ import division, print_function\n",
    "%matplotlib inline\n",
    "import sys\n",
    "sys.path.insert(0,'..') # allow us to format the book\n",
    "sys.path.insert(0,'../code') \n",
    "\n",
    "# use same formatting as rest of book so that the plots are\n",
    "# consistant with that look and feel.\n",
    "import book_format\n",
    "book_format.load_style(directory='..')\n",
    "\n",
    "\n",
    "\n",
    "def update(mu1, var1, mu2, var2):\n",
    "    mean = (var1*mu2 + var2*mu1) / (var1+var2)\n",
    "    variance = 1 / (1/var1 + 1/var2)\n",
    "    return (mean, variance)\n",
    "\n",
    "def predict(pos, variance, movement, movement_variance):\n",
    "    return (pos + movement, variance + movement_variance)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This notebook creates the animations for the Kalman Filter chapter. It is not really intended to be a readable part of the book, but of course you are free to look at the source code, and even modify it. However, if you are interested in running your own animations, I'll point you to the examples subdirectory of the book, which contains a number of python scripts that you can run and modify from an IDE or the command line. This module saves the animations to GIF files, which is quite slow and not very interactive. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def plot_3d_covariance(ax, mean, cov):\n",
    "    \"\"\" plots a 2x2 covariance matrix positioned at mean. mean will be plotted\n",
    "    in x and y, and the probability in the z axis.\n",
    "\n",
    "    Parameters\n",
    "    ----------\n",
    "    mean :  2x1 tuple-like object\n",
    "        mean for x and y coordinates. For example (2.3, 7.5)\n",
    "\n",
    "    cov : 2x2 nd.array\n",
    "       the covariance matrix\n",
    "\n",
    "    \"\"\"\n",
    "\n",
    "    # compute width and height of covariance ellipse so we can choose\n",
    "    # appropriate ranges for x and y\n",
    "    o,w,h = stats.covariance_ellipse(cov,3)\n",
    "    # rotate width and height to x,y axis\n",
    "    wx = abs(w*np.cos(o) + h*np.sin(o))*1.2\n",
    "    wy = abs(h*np.cos(o) - w*np.sin(o))*1.2\n",
    "\n",
    "\n",
    "    # ensure axis are of the same size so everything is plotted with the same\n",
    "    # scale\n",
    "    if wx > wy:\n",
    "        w = wx\n",
    "    else:\n",
    "        w = wy\n",
    "\n",
    "    minx = mean[0] - w\n",
    "    maxx = mean[0] + w\n",
    "    miny = mean[1] - w\n",
    "    maxy = mean[1] + w\n",
    "\n",
    "    xs = np.arange(minx, maxx, (maxx-minx)/40.)\n",
    "    ys = np.arange(miny, maxy, (maxy-miny)/40.)\n",
    "    xv, yv = np.meshgrid (xs, ys)\n",
    "\n",
    "    zs = np.array([100.* stats.multivariate_gaussian(np.array([x,y]),mean,cov) \\\n",
    "                   for x,y in zip(np.ravel(xv), np.ravel(yv))])\n",
    "    zv = zs.reshape(xv.shape)\n",
    "\n",
    "    ax = plt.figure().add_subplot(111, projection='3d')\n",
    "    ax.plot_surface(xv, yv, zv, rstride=1, cstride=1, cmap=cm.autumn)\n",
    "\n",
    "    ax.set_xlabel('X')\n",
    "    ax.set_ylabel('Y')\n",
    "\n",
    "    ax.contour(xv, yv, zv, zdir='x', offset=minx-1, cmap=cm.autumn)\n",
    "    ax.contour(xv, yv, zv, zdir='y', offset=maxy, cmap=cm.BuGn)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "ename": "ModuleNotFoundError",
     "evalue": "No module named 'gif_animate'",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mModuleNotFoundError\u001b[0m                       Traceback (most recent call last)",
      "\u001b[1;32m<ipython-input-3-42bb287cbba9>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m\u001b[0m\n\u001b[0;32m      4\u001b[0m \u001b[1;32mimport\u001b[0m \u001b[0mmath\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      5\u001b[0m \u001b[1;32mimport\u001b[0m \u001b[0mstats\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m----> 6\u001b[1;33m \u001b[1;32mfrom\u001b[0m \u001b[0mgif_animate\u001b[0m \u001b[1;32mimport\u001b[0m \u001b[0manimate\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m      7\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      8\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;31mModuleNotFoundError\u001b[0m: No module named 'gif_animate'"
     ]
    }
   ],
   "source": [
    "from __future__ import print_function, division\n",
    "import matplotlib.pyplot as plt\n",
    "import numpy.random as random\n",
    "import math\n",
    "import stats\n",
    "from gif_animate import animate\n",
    "\n",
    "\n",
    "\n",
    "sensor_error = 1.2**2\n",
    "movement_error = .2\n",
    "movement = 0\n",
    "voltage = (25,20) #who knows what the first value is?\n",
    "\n",
    "volts = [14.44, 18.74, 17.21, 15.98, 16.76, 14.8, 16.01, 16.90, 15.41, 17.31,\n",
    "        16.39, 17.96]\n",
    "ps = [voltage[0]]\n",
    "\n",
    "\n",
    "i = 0\n",
    "pred = 0\n",
    "def volt_animate(frame):\n",
    "    global i, ps, voltage, Z, x, pred\n",
    "    \n",
    "    step = frame % 4\n",
    "    plt.subplot(211)\n",
    "    plt.ylim([14,26])\n",
    "\n",
    "    if step == 0:\n",
    "        prev = voltage[0]\n",
    "        voltage = predict(voltage[0], voltage[1], movement, movement_error)\n",
    "        pred = voltage[0]\n",
    "        plt.plot ([i, i+1], [prev, pred], c='g') \n",
    "        \n",
    "    elif step == 1:\n",
    "        Z = volts[i]\n",
    "        i += 1\n",
    "        \n",
    "        plt.scatter(i, Z, marker='+', s=64, color='r')\n",
    "\n",
    "    elif step == 2:\n",
    "        plt.plot ([i,i], [pred, Z], c='r', alpha=0.3)\n",
    "\n",
    "    else:\n",
    "        voltage = update(voltage[0], voltage[1], Z, sensor_error)\n",
    "        ps.append(voltage[0])\n",
    "        plt.plot(ps, c='b')\n",
    "        \n",
    "    plt.subplot(212)\n",
    "    plt.cla()\n",
    "    stats.plot_gaussian(voltage[0], voltage[1], xlim=[0,32])\n",
    "    plt.ylim([0,1])\n",
    "    plt.tight_layout()\n",
    "        \n",
    "N=12\n",
    "animate('05_volt_animate.gif', volt_animate, N*3, 350)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src='05_volt_animate.gif'>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "ename": "ModuleNotFoundError",
     "evalue": "No module named 'gif_animate'",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mModuleNotFoundError\u001b[0m                       Traceback (most recent call last)",
      "\u001b[1;32m<ipython-input-4-0138411cd6ba>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m\u001b[0m\n\u001b[0;32m      4\u001b[0m \u001b[1;32mimport\u001b[0m \u001b[0mmath\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      5\u001b[0m \u001b[1;32mimport\u001b[0m \u001b[0mstats\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m----> 6\u001b[1;33m \u001b[1;32mfrom\u001b[0m \u001b[0mgif_animate\u001b[0m \u001b[1;32mimport\u001b[0m \u001b[0manimate\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m      7\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      8\u001b[0m \u001b[1;31m# assume dog is always moving 1m to the right\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;31mModuleNotFoundError\u001b[0m: No module named 'gif_animate'"
     ]
    }
   ],
   "source": [
    "from __future__ import print_function, division\n",
    "import matplotlib.pyplot as plt\n",
    "import numpy.random as random\n",
    "import math\n",
    "import stats\n",
    "from gif_animate import animate\n",
    "\n",
    "# assume dog is always moving 1m to the right\n",
    "movement = 1\n",
    "movement_error = .05\n",
    "sensor_error = 4.5\n",
    "pos = (0, 100)   # gaussian N(0,100)\n",
    "\n",
    "# this is the recorded output of a run of the dog sensor with an initial\n",
    "# seed of 200\n",
    "ZS = [-2.07, 6.05, 4.51, 3.47, 5.76, 5.93, 6.53, 9.01, 7.53, 11.68, \n",
    "      11.15, 14.76, 13.45, 16.15, 19.05, 14.87, 20.90, 15.75, \n",
    "      17.16, 20.50]\n",
    "zs = []\n",
    "ps = []\n",
    "\n",
    "N=20\n",
    "\n",
    "def dog_animate(frame):\n",
    "    global pos, zs, ps,  N, ZS\n",
    "    pos = predict(pos[0], pos[1], movement, movement_error)    \n",
    "    Z = ZS[frame]\n",
    "    zs.append(Z)\n",
    "    \n",
    "    pos = update(pos[0], pos[1], Z, sensor_error)\n",
    "    ps.append(pos[0])\n",
    "\n",
    "\n",
    "    plt.subplot(211)\n",
    "    plt.plot(zs,c='r', linestyle='dashed')\n",
    "\n",
    "    plt.xlim([0,N*1.2])\n",
    "    plt.ylim([0,N*1.2])\n",
    "\n",
    "    if len(ps) > 1:\n",
    "        plt.plot(ps, c='#8EBA42')\n",
    "        \n",
    "    plt.subplot(212)\n",
    "    plt.cla()\n",
    "    stats.plot_gaussian(pos[0], pos[1], xlim=[0,N*1.2])\n",
    "    plt.ylim(0, 1)\n",
    "    plt.tight_layout()\n",
    "\n",
    "\n",
    "animate('05_dog_track.gif', dog_animate, N, 200)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src='05_dog_track.gif'>"
   ]
  }
 ],
 "metadata": {
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
